
function MainStep3_MultiShoot
%
% 多步打靶（LEO -> LLO）
%
% 输出：
% converged_LEO2LLO.mat
%
% 作者：张晨
% 邮箱：chenzhang@csu.ac.cn
% 单位：中国科学院空间应用工程与技术中心，空间探索室
% 时间：2022年02月24日
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clc; clear; close all; clear path;
% restoredefaultpath;
warning off;

% 添加模块
parentFolder = fileparts(pwd);
addpath(genpath([parentFolder , '\Basic']));
addpath(genpath([parentFolder , '\Bcr4bp']));
addpath(genpath([parentFolder , '\Cr3bp']));

% 载入参数
aux = Bcr4bp_Aux;

% 载入上一步结果
load picked_LEO2LLO;

% -------------------------------------- 多步打靶 ------------------------------------------------
% 构造多步打靶初值，并画图
yy = getIC(picked_LEO2LLO , aux);

opts = optimoptions(@fmincon , 'Algorithm' , 'active-set' , 'Display' , 'iter' , ...
    'MaxIter' , 20 , 'MaxFunEvals' , 200 , ...
    'GradObj' , 'on' , 'GradConstr' , 'on' , 'DerivativeCheck' , 'off');

try
    [yy , fobj , ExitFlag] = fmincon(@Bcr4bp_Obj , yy , [] , [] , [] , [] , [] , [] , @Bcr4bp_ConsEMRot2D , opts , aux);
catch
    fprintf('不收敛，程序中止！请更换其他初值！\n')
end

% ------------------------------------ 收敛轨道数值积分 ------------------------------------
xx1 = yy(1 : 4);
xxf = yy(4 * (aux.nNode - 1) + 1 : 4 * aux.nNode);
tau1 = yy(4 * aux.nNode + 1);
tauf = yy(4 * aux.nNode + 2);

options = odeset('Reltol' , aux.tol , 'AbsTol' , aux.tol);
[seg_tt , seg_xx] = ode113(@Bcr4bp_EqmEMRot2D , [tau1 , tauf] , [xx1] , options , aux);

% -------------------- 画EMRot轨道 --------------------
figure(1)
plotOrb2D(seg_xx , 'r' , 2)

% -------------------- 画SB1Rot轨道 --------------------
figure(2);
[tau_SB1Rot , xb2c_SB1Rot] = Bcr4bp_EMRot2SB1RotMtx2D(seg_tt , seg_xx , aux);
plot(xb2c_SB1Rot(: , 1) , xb2c_SB1Rot(: , 2) , 'r' , 'LineWidth' , 2);

% -------------------- 保存结果 --------------------
converged_LEO2LLO = [seg_tt(1) , seg_xx(1 , :) , seg_tt(end) , seg_xx(end , :)];
save converged_LEO2LLO converged_LEO2LLO

% ----------------------------------------------------- 暂存，勿删 -----------------------------------------------------
% err = inf;
% iterNum = 1;
% iterNumMax = 20;
% while err > 1e-8 && iterNum < iterNumMax
%
%     % 约束 / 约束梯度
%     [~, ceq, ~, Gceq] = Bcr4bp_ConsEMRot2D(yy , aux);
%     Gceq = Gceq';
%
%     % 计算误差
%     err = max(abs(ceq));
%     fprintf('误差: %0.4e \n' , err);
%
%     %     % 能否求逆
%     %     if det(Gceq * Gceq') < 1e-6
%     %         err_flag = 0;
%     %         fprintf('不能求逆！')
%     %         break;
%     %     end
%
%     % 打靶变量更新
%     % yy = yy - Gceq' * inv(Gceq * Gceq') * ceq;
%     yy = yy - Gceq' / (Gceq * Gceq') * ceq;
%     iterNum = iterNum + 1;
%
% end

end
